
#include <BPainter>
#include <BRender>
#include <BViewer>
#include <BScene>
#include <BPhysics>
#include <BCone>
#include <BCube>
#include <BImage>
#include <BMaterial>
#include <BReadWrite>

#include "SixdofHandler.h"

SixdofHandler::SixdofHandler()
{
	BImage* image = ReadFile<BImage>("data/image/whitemetal_diffuse.jpg");

	_nodeA = new BNode("nodeA");
	{
		BPhysics* physics = new BPhysics();
		physics->setMass(0.0);
		_nodeA->setPhysics(physics);

		BCube* cube = new BCube(4, 12);
		cube->setColor(100, 200, 255);
		_nodeA->addShape(cube);
	}

	_nodeB = new BNode("nodeB");
	{
		BPhysics* physics = new BPhysics();
		physics->setMass(1.0);
		_nodeB->setPhysics(physics);

		BCone* cone = new BCone(0.5, 2, 24, 10);
		cone->setTexture(image);
		cone->setColor(200, 100, 0);
		_nodeB->addShape(cone);
	}

	BVector pointA = _nodeA->space().max() + BVector(0, 0, 1);
	BVector pointB(0, 0, _nodeB->space().max().z());
	_constraint = new BSixdofConstraint(_nodeA, pointA, _nodeB, pointB);
	_constraint->setLinearLowerLimit(-1.0, -0.5, 0.0);
	_constraint->setLinearUpperLimit(1.0, 1.0, 0.0);
	_constraint->setAngularLowerLimit(0, 0, -PI);
	_constraint->setAngularUpperLimit(0, 0, PI);
	_constraint->setName("Sixdof");
}
SixdofHandler::~SixdofHandler()
{

}

void SixdofHandler::attached(BViewer* viewer)
{
	BScene* dscene = viewer->scene();
	_nodeA->setPosition(0, 0, 5);
	_nodeB->setPosition(2, 2, 5);
	dscene->insert(_nodeA);
	dscene->insert(_nodeB);
	dscene->insert(_constraint);
	this->emit("edit-constraint", _constraint);
}
void SixdofHandler::detached(BViewer* viewer)
{
	BScene* dscene = viewer->scene();
	dscene->remove(_nodeA);
	dscene->remove(_nodeB);
	dscene->remove(_constraint);
}

void SixdofHandler::paint(BPainter& painter)
{
	BViewer* viewer = this->viewer();
	painter.setLineWidth(10);
	painter.setColor(BColor(0, 1, 0));
	painter.drawRect(10, 10, viewer->width() - 20, viewer->height() - 20);


}
void SixdofHandler::render(BRender& render)
{
	render.setLighting(false);
	render.setDepthTest(false);
	render.setLineSmooth(true);
	render.setLineWidth(1);
	
	if (_constraint)
	{
		BVector origin = _constraint->pointA();

		const BVector& linearLowerLimit = _constraint->linearLowerLimit();
		const BVector& linearUpperLimit = _constraint->linearUpperLimit();

		BVector pminx = (origin + BVector(linearLowerLimit.x(), 0, 0)) * _constraint->nodeA()->matrix();
		BVector pmaxx = (origin + BVector(linearUpperLimit.x(), 0, 0)) * _constraint->nodeA()->matrix();
		BVector pminy = (origin + BVector(0, linearLowerLimit.y(), 0)) * _constraint->nodeA()->matrix();
		BVector pmaxy = (origin + BVector(0, linearUpperLimit.y(), 0)) * _constraint->nodeA()->matrix();
		BVector pminz = (origin + BVector(0, 0, linearLowerLimit.z())) * _constraint->nodeA()->matrix();
		BVector pmaxz = (origin + BVector(0, 0, linearUpperLimit.z())) * _constraint->nodeA()->matrix();
		origin = origin * _constraint->nodeA()->matrix();
		render.setColor(255, 100, 0);
		render.drawLine(pminx, pmaxx);
		render.setColor(0, 255, 100);
		render.drawLine(pminy, pmaxy);
		render.setColor(0, 100, 255);
		render.drawLine(pminz, pmaxz);

		BVector pointB = _constraint->nodeB()->matrix() * _constraint->pointB();

		return;
	}

}
